/*
 * example7.c
 *
 * Created: 8/29/2013 11:41:01 PM
 *  Author: Tim
 */ 


#include "src/Epiphany.h"

uint16_t i;

void setMotorEffort(uint8_t motorNumber, uint16_t effort, uint8_t direction);
		
void setup()
{
	//initialize system clock to 32MHz
	clockInit();

	
	RTC_DelayInit();

	//global interrupt enable
	sei();
	
}

void loop()
{
	for(i=0;i<1024;)
	{
		setMotorEffort(0,i,0);
		
		if(delayOver)
		{
			RTC_Delay_ms(1);
			i++;
		}
	}
	
	for(i=1024;i>0;)
	{
		setMotorEffort(0,i,0);
		
		if(delayOver)
		{
			RTC_Delay_ms(1);
			i--;
		}
	}
	
		for(i=0;i<1024;)
		{
			setMotorEffort(0,i,1);
			
			if(delayOver)
			{
				RTC_Delay_ms(1);
				i++;
			}
		}
		
		for(i=1024;i>0;)
		{
			setMotorEffort(0,i,1);
			
			if(delayOver)
			{
				RTC_Delay_ms(1);
				i--;
			}
		}

}

void motorSetup()
{	
	TCD0.CTRLA		=	TC_CLKSEL_DIV1_gc;
	TCD0.PER		=	1024;
	TCD0.CTRLB		= 
						TC0_CCAEN_bm	|
						TC0_CCBEN_bm	|
						TC0_CCCEN_bm	|
						TC0_CCDEN_bm	|
						TC_WGMODE_SS_gc	;
	
	PORTD.DIRSET	=	0x3F;
	
	TCD0.CCA		=	0;
	TCD0.CCB		=	0;
}

void setMotorEffort(uint8_t motorNumber, uint16_t effort, uint8_t direction)
{
	if(motorNumber > 1)motorNumber = 1;
	
	if(motorNumber)
	{
		if(direction)
		{
			PORTD.OUTCLR = 0x30;
			PORTD.OUTSET = 0x10; 
		}
		else
		{
			PORTD.OUTCLR = 0x30;
			PORTD.OUTSET = 0x20;
		}
		TCD0.CCB = effort;
	}
	else
	{
		if(direction)
		{
			PORTD.OUTCLR = 0x0C;
			PORTD.OUTSET = 0x08;
		}
		else
		{
			PORTD.OUTCLR = 0x0C;
			PORTD.OUTSET = 0x04;
		}
		TCD0.CCA = effort;
	}
}